教程_如何控制Franka机械臂在仿真环境下运动

本文档参考了franka官方教程和moveit!官方教程撰写。

1.环境安装简介

本例中至少需要安装：Ubuntu、ROS、franka_ros、moveit!，此处不再赘述安装过程，仅给出网络上的安装教程：

• Ubuntu（此处推荐Ubuntu20.04）：虚拟机安装Ubuntu教程，若在真实物理PC上安装，仅需省略教程中虚拟机的那一部分，其他完全一样
• ROS（此处推荐Noetic）：鱼香ROS提供的一键安装指令：链接，为了避免后续出现问题，此处推荐也对rosdep进行配置
• franka_ros：franka官方教程
• moveit!：moveit!官方教程

2.安装环境ready验证

2.1 验证gazebo和rvize环境ready

在命令行中运行下述命令以添加环境变量：

source /path/to/catkin_ws/devel/setup.sh

此处的path为franka的工作空间，关于工作空间，参考官方链接--构建ROS包一节。 再运行：

roslaunch franka_gazebo panda.launch x:=-0.5 \
    world:=$(rospack find franka_gazebo)/world/stone.sdf \
    controller:=cartesian_impedance_example_controller \
    rviz:=true

此时若环境已完全安装，则可以看到带有石头和RViz的环境。若此时可以通过在rviz中用鼠标拖动末端来控制机器人位姿，则说明ros和rviz已ready。

重新打开一个终端，添加环境变量后运行：

rostopic pub --once /franka_gripper/move/goal franka_gripper/MoveActionGoal "goal: { width: 0.08, speed: 0.1 }"

此时若可以看到，Rviz和gazebo中的机械臂夹爪均已打开，则说明gazebo已ready

2.2 验证moveit!环境ready

打开一个终端，添加环境变量后，执行下述指令：

roslaunch panda_moveit_config demo.launch

再另一个终端中，执行下述指令：

roslaunch moveit_tutorials move_group_interface_tutorial.launch

注意：官方的教程中用到了RvizVisualToolGui来控制机械臂，若在顶部Panels->Add New Panel中找不到这一选项，则需要参考此处的教程先添加这一插件。

添加插件后，可以通过底部的next来控制demo依次运行，若demo运行成功，则moveit!环境已ready

3.使用自定义测试用例控制

控制franka机械臂运动的方式多种多样，此处仅介绍使用moveit!对franka进行开发和控制的方法。注意：这需要对ros和moveit!有一定了解。

1）新建工作空间catkin_ws

2）在工作空间下新建功能包moveit_user

3）在src路径下新建测试用例文件test_joint_move.cpp

4）编辑CMakeList.txt与package.xml文件

add_executable(test_joint_move src/test_joint_move.cpp)
target_link_libraries(test_joint_move ${catkin_LIBRARIES} ${Boost_LIBRARIES})
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

在CMakeLists中的find_package中加入：moveit_ros_planning_interface

在package.xml中加入moveit_ros_planning_interface作为依赖

5）编译文件，检查是否存在配置错误

catkin make

此处moveit!官方推荐用catkin make而非catkin_build编译

6）编写test_joint_move.cpp，实现功能

#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char* argv[])
{
    ros::init(argc, argv, "test_joint_move");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    static const std::string PLANNING_GROUP = "panda_arm";
    moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);
    std::vector<double> joint_target = move_group_interface.getCurrentJointValues();

    if (argc == 3)
    {
        int i = atoi(argv[1]);
        joint_target[i] = atof(argv[2]);
    }
    else
    {
        for(int i = 0; i < argc - 1; i++)
        {
            joint_target[i] = atof(argv[i + 1]);
        }
    }
    move_group_interface.setJointValueTarget(joint_target);
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    moveit::core::MoveItErrorCode success = move_group_interface.plan(my_plan);
    if (success.val) 
    {
        move_group_interface.execute(my_plan);
        ROS_INFO("move successfully, ret = %d",success.val);
    }
    else
    {
        ROS_ERROR("failed to plan motion, ret = %d",success.val);
    }
    std::vector<double> joint_current = move_group_interface.getCurrentJointValues();
    std::stringstream ss;
    for(std::size_t i = 0; i < joint_current.size(); ++i)
    {
        ss << "Joint " << i + 1 << ": " << joint_current[i] << "\n";
    }
    ROS_INFO_STREAM(ss.str());
    ros::shutdown();
    return 0;
}

7）编译文件，检查代码是否存在错误

catkin make

8）在命令行执行test_joint_move节点，观察输出结果

先打开一个终端，添加环境变量后，执行下述指令启动仿真环境：

roslaunch panda_moveit_config demo.launch

再打开一个终端，添加环境变量后，执行下述指令运行节点：

rosrun moveit_user test_joint_move 0 1.5708

此时可以看到，RViz中的机械臂规划出了一条半透明的白色轨迹，然后关节1运动90deg到目标位置。Rviz界面和终端输出如下：

4.遗留问题

• 目前仅能够控制机械臂进行正逆运动学测试，力控尚未实现
• 即便使用官方的测试用例，也存在机械臂规划后在RViz中显示运动时卡顿现象，不明原因